//if it starts with a lower case letter it is a private and starts with an upper case letter it is public 
#include <stdlib.h>
#include <math.h>
#include "Lift.h"
#include "Victor.h"

bool
Lift::isNearEnough(void)
{
	return (abs(m_armHeight->GetValue() - m_targetHeight) <= m_heightLeeway);
}

void
Lift::setMotors(float value)
{	
#if 0
	if((m_armHeight->GetValue() >= m_maxHeight) || (m_armHeight->GetValue() <= m_minHeight))
		return;
#endif
	
	m_leftMotor->Set(value);
	if(value > kNeutralValue)				//Only run the right motor if going up
		m_rightMotor->Set(kNeutralValue);
	else
		m_rightMotor->Set(value);
}

Lift::Lift(unsigned int leftMotorChannel, unsigned int rightMotorChannel, unsigned int armHeightChannel,
				bool invertLeft, bool invertRight, int heightLeeway)
{	                                                            
	//TODO: Replace with EEPROM query.
	m_maxHeight = 0;
	m_minHeight = 0;
	
	m_leftMotor  = new Victor(leftMotorChannel);
	m_rightMotor = new Victor(rightMotorChannel);//GRRRR this is product placement >.<
	
	m_armHeight    = new AnalogChannel(armHeightChannel);
	
	m_invertLeft   = invertLeft;
	m_invertRight  = invertRight;
	m_heightLeeway = heightLeeway;
}

void
Lift::SetLimits(int maxHeight, int minHeight)
{
	m_maxHeight = maxHeight;
	m_minHeight = minHeight;
}

void
Lift::DriveTo(signed int height)
{
	m_targetHeight = height;
	
	if(isNearEnough())
		setMotors(kNeutralValue);
	else if(m_targetHeight > m_armHeight->GetValue())
		setMotors(kLiftValue);
	else if(m_targetHeight < m_armHeight->GetValue())
		setMotors(kDropValue);
}

void
Lift::ManualDrive(float speed)
{
	setMotors(speed);
}

signed int
Lift::GetCurrentHeight(void)
{
	return m_armHeight->GetValue();
}

bool
Lift::IsAt(signed int height)
{
	return isNearEnough();
}
